//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <ddk.h>
#include <bulverde.h>
#include "drvpub.h"

//#define DEBUG_DRVBUF
#ifdef DEBUG_DRVBUF
#define DBGOUT(x) (x)
#else
#define DBGOUT(x)
#endif

#define CLK_SRAM 0x00100000

#define SDRAM_PRIVATE_START 0xA1E00000 // from 30M, TOTAL 2M
#define SRAM_START          0x5C000000 // SRAM (TOTAL 256KB)
#define SDRAM_PRIVATE_SIZE  0x200000
#define SRAM_SIZE           0x40000

#define DRVBUFFER_MAXPAGES 4096 // 16M
#define BIGBUFFER_MINPAGES 16 // 64K

typedef struct _DrvBuffer {
    uint_t  uBitMap[DRVBUFFER_MAXPAGES >> 5];
    uint_t  uTotalPages;
    uint_t  uFreePages;
    uint_t  physBase;
    uint_t  virtBase;
    DzMutex   *pBufferLock;
} DrvBuffer;
static DrvBuffer * s_pDrvBuf; // SDRAM
static DrvBuffer * s_pDrvBufFast; // SRAM
static DzMutex  s_bufferLock;
//static DzMutex  s_bufferLock1;

#ifdef DEBUG_DRVBUF
static void DumpMemMap(DrvBuffer *pDrvBuf)
{
    uint_t i = 0;

    WaitResult wr;
    pDrvBuf->pBufferLock->Lock(&wr);

    uint_t uSize = pDrvBuf->uTotalPages >> 5;
    uint_t *pMap = pDrvBuf->uBitMap;

    kprintf("\n========Start Dump Driver Buffer========\n");
    kprintf("pDrvBuf: 0x%x\n", pDrvBuf);

    kprintf("uBitMap:  0x%x, Content:\n", pDrvBuf->uBitMap);
    do {
        kprintf("   %08x", pMap[i]);
        i++;
        if ((i & 0x3) == 0) kprintf("\n");
    } while (i < uSize);
    kprintf("\nuTotalPages: %d\n", pDrvBuf->uTotalPages);
    kprintf("uFreePages: %d\n", pDrvBuf->uFreePages);
    kprintf("physBase: 0x%x\n", pDrvBuf->physBase);
    kprintf("virtBase: 0x%x\n", pDrvBuf->virtBase);
    kprintf("========Finish Dump Driver Buffer Map========\n");

    pDrvBuf->pBufferLock->Unlock();
}

static void DumpMemMaps()
{
    DumpMemMap(s_pDrvBuf);
    DumpMemMap(s_pDrvBufFast);
}
#endif

INLINE void InitDrvBuffer(DrvBuffer *pDrvBuf, uint_t uPages,
    uint_t physAddr, uint_t virtAddr)
{
    pDrvBuf->uTotalPages = uPages;
    pDrvBuf->uFreePages = uPages;
    pDrvBuf->physBase = physAddr;
    pDrvBuf->virtBase = virtAddr;
    memset(pDrvBuf->uBitMap, 0, uPages >> 3);
    pDrvBuf->pBufferLock = &s_bufferLock;
}

// If the bits are 1, then it should be 0.
// If the bits are 0, then it should be 1.
INLINE void ChangeMemMap(uint_t *pMap, int i, int j, uint_t uPages)
{
    // Change the pages represented by pMap[i]
    uint_t temp = 32 - j;
    if (uPages >= temp) {
        pMap[i++] ^= 0xFFFFFFFF << j;
        uPages -= temp;
    }
    else {
        pMap[i] ^= (0xFFFFFFFF << j) & (0xFFFFFFFF >> (temp - uPages));
        return;
    }

    // Change 32 pages each time
    while (uPages > 32) {
        pMap[i++] ^= 0xFFFFFFFF;
        uPages -= 32;
    }

    // Change less than 32 pages
    pMap[i] ^= ~(0xFFFFFFFF << uPages);

    return;
}

// Search smaller buffer here
static int FindPagesFromLowAddr(DrvBuffer *pDrvBuf, uint_t uPages)
{
    uint_t uFoundPages = 0;
    uint_t i = 0, iMax = pDrvBuf->uTotalPages >> 5;
    uint_t *pMap = pDrvBuf->uBitMap;

    uint_t val = pMap[i];
    int Start_i = 0, Start_j = 0;
    do {
        int j = 0;
        val = pMap[i];

        while (j < 32) {
            if (((val >> j) & 0x1) == 0) {
                if (uFoundPages == 0) {
                    Start_i = i;
                    Start_j = j;
                }
                uFoundPages++;
                if (uFoundPages == uPages) {
                    // Found it here!
                    ChangeMemMap(pMap, Start_i, Start_j, uPages);
                    pDrvBuf->uFreePages -= uPages;
                    return (Start_i << 5) + Start_j;
                }
            }
            else uFoundPages = 0;
            j++;
        }
        i++;
    } while (i < iMax);
    return -1;
}

// Search bigger buffer here
static int FindPagesFromHighAddr(DrvBuffer *pDrvBuf, uint_t uPages)
{
    uint_t uFoundPages = 0;
    int i = (pDrvBuf->uTotalPages >> 5) - 1;
    uint_t *pMap = pDrvBuf->uBitMap;

    uint_t val;
    do {
        val = pMap[i];
        int j = 31;

        while (j >= 0) {
            if (((val >> j) & 0x1) == 0) {
                uFoundPages++;
                if (uFoundPages == uPages) {
                    // Found it here!
                    ChangeMemMap(pMap, i, j, uPages);
                    pDrvBuf->uFreePages -= uPages;
                    return (i << 5) + j;
                }
            }
            else uFoundPages = 0;
            j--;
        }
        i--;
    } while (i >= 0);
    return -1;
}

INLINE DrvBuffer * GetDrvBuf(void * VirtAddr)
{
    if ((uint_t)VirtAddr - (uint_t)s_pDrvBuf->virtBase <
        (s_pDrvBuf->uTotalPages << PAGE_SHIFT)) {
        return s_pDrvBuf;
    }
    else {
        Drv_Assert((uint_t)VirtAddr - (uint_t)s_pDrvBufFast->virtBase <
            (s_pDrvBufFast->uTotalPages << PAGE_SHIFT));
        return s_pDrvBufFast;
    }
}

EXTERN_C ECode Drv_InitBuffer()
{
    virtaddr_t virtBase, virtBaseFast;

    Drv_Assert (2 * sizeof(DrvBuffer) <= PAGE_SIZE); // One page can hold 2 DrvBuffer struct

    virtBase = DzKIoRemap(SDRAM_PRIVATE_START, SDRAM_PRIVATE_SIZE);
    Drv_Assert(virtBase && "KIoRemap virtBase failed!");

    virtBaseFast = DzKIoRemap(SRAM_START, SRAM_SIZE);
    Drv_Assert(virtBaseFast && "KIoRemap virtBaseFast failed!");

    s_pDrvBuf = (DrvBuffer *)virtBase;
    s_pDrvBufFast = s_pDrvBuf + 1;

    InitDrvBuffer(s_pDrvBuf, SDRAM_PRIVATE_SIZE >> PAGE_SHIFT,
        SDRAM_PRIVATE_START, (uint_t)virtBase);
    InitDrvBuffer(s_pDrvBufFast, SRAM_SIZE >> PAGE_SHIFT,
        SRAM_START, (uint_t)virtBaseFast);

    s_pDrvBuf->uBitMap[0] |= 0x1; // First page reserved for DrvBuffer struct
    s_pDrvBuf->uFreePages--;

    DBGOUT(DumpMemMaps());
    return NOERROR;
}

EXTERN_C void * Drv_AllocBuffer(uint_t uPages, uint_t uFlag)
{
    int nPageoffset;
    void *VirtAddr;
    DrvBuffer *pDrvBuf;
    Boolean bOpenSRAM = FALSE;

    if (uFlag & DRVBUF_SPEED_FAST) {
        pDrvBuf = s_pDrvBufFast;
        if (pDrvBuf->uTotalPages == pDrvBuf->uFreePages) {
             bOpenSRAM = TRUE;
        }
    }
    else {
        pDrvBuf = s_pDrvBuf;
    }

    Drv_Assert(pDrvBuf);

    DBGOUT(kprintf("Start Drv_AllocBuffer, uFlag = 0x%08x, uPages = %d\n", uFlag, uPages));

    if (0 == uPages) return NULL;
    if (pDrvBuf->uFreePages < uPages) goto Error;

    WaitResult wr;
    pDrvBuf->pBufferLock->Lock(&wr);

    // Search the bitmap for memory
    if (uPages <=  BIGBUFFER_MINPAGES) {
        nPageoffset = FindPagesFromLowAddr(pDrvBuf, uPages);
    }
    else {
        nPageoffset = FindPagesFromHighAddr(pDrvBuf, uPages);
    }

    pDrvBuf->pBufferLock->Unlock();

    if (-1 == nPageoffset) goto Error;

    if (bOpenSRAM) CKEN |= CLK_SRAM; // Open SRAM clock

    VirtAddr = (void *)((uint_t)pDrvBuf->virtBase + (nPageoffset << PAGE_SHIFT));

    DBGOUT(DumpMemMaps());
    DBGOUT(kprintf("Finish Drv_AllocBuffer VirtAddr = 0x%08x\n", VirtAddr));

    return VirtAddr;
Error:
    DBGOUT(kprintf("Finish Drv_AllocBuffer, no buffer to allocate.\n"));
    return NULL;
}

EXTERN_C void * Drv_ReallocBuffer(void * VirtAddr, uint_t uOldPages,
    uint_t uNewPages, uint_t uFlag)
{
    Boolean bMoveMemory = FALSE;
    uint_t uPageoffset = 0, uChangedPages = 0;
    void *pNewBuffer = NULL;
    DrvBuffer *pDrvBuf;

    // Free buffer
    if (0 == uNewPages) {
        Drv_FreeBuffer(VirtAddr, uOldPages);
        return NULL;
    }

    // Allocate a new buffer
    if (!VirtAddr) return Drv_AllocBuffer(uNewPages, uFlag);

    pDrvBuf = GetDrvBuf(VirtAddr);
    if (pDrvBuf == s_pDrvBuf && (uFlag & DRVBUF_SPEED_FAST)) bMoveMemory = TRUE;
    if (pDrvBuf == s_pDrvBufFast && !(uFlag & DRVBUF_SPEED_FAST)) bMoveMemory = TRUE;

    DBGOUT(kprintf("Start Drv_ReallocBuffer, VirtAddr = 0x%08x, uOldPages = %d, ",
        VirtAddr, uOldPages));
    DBGOUT(kprintf(" uNewPages = %d, uFlag = 0x%08x, bMoveMemory = %d\n",
        uNewPages, uFlag, bMoveMemory));

    uPageoffset = ((uint_t)VirtAddr - pDrvBuf->virtBase) >> PAGE_SHIFT;
    Drv_Assert((pDrvBuf->uBitMap[uPageoffset >> 5] >> (uPageoffset & 0x1F)) & 0x1);
    if (!bMoveMemory) {
        if (uNewPages == uOldPages) return  VirtAddr;

        if (uNewPages < uOldPages) {
            uChangedPages = uOldPages - uNewPages;
            uPageoffset += uNewPages;
            pNewBuffer = VirtAddr;
            goto ChangeMap;
        }
    }

    //
    // Change memory
    //

    // First, find the uNewPages
    pNewBuffer = Drv_AllocBuffer(uNewPages, uFlag);
    if (!pNewBuffer) {
        return NULL;
    }
    if (uFlag & DEVBUF_REALLOC_COPY) {
        // Move buffer content to the new place
        memcpy(pNewBuffer, VirtAddr,
            (uOldPages < uNewPages ? uOldPages : uNewPages) << PAGE_SHIFT);
    }
    uChangedPages = uOldPages;

ChangeMap:
    WaitResult wr;
    pDrvBuf->pBufferLock->Lock(&wr);
    ChangeMemMap(pDrvBuf->uBitMap, uPageoffset >> 5,
                uPageoffset & 0x1F, uChangedPages);
    pDrvBuf->uFreePages += uChangedPages;
    if (pDrvBuf == s_pDrvBufFast) {
        if (pDrvBuf->uTotalPages == pDrvBuf->uFreePages) {
            CKEN &= ~CLK_SRAM; // Close SRAM clock
        }
    }
    pDrvBuf->pBufferLock->Unlock();
    DBGOUT(DumpMemMaps());
    DBGOUT(kprintf("Finish Drv_ReallocBuffer pNewBuffer = 0x%08x\n", pNewBuffer));
    return pNewBuffer;
}

EXTERN_C void Drv_FreeBuffer(void * VirtAddr, uint_t uPages)
{
    DrvBuffer *pDrvBuf;
    uint_t uPageoffset;

    DBGOUT(kprintf("Start Drv_FreeBuffer, VirtAddr = 0x%08x, uPages = %d\n", VirtAddr, uPages));

    if (!VirtAddr || 0 == uPages) return;
    pDrvBuf = GetDrvBuf(VirtAddr);
    uPageoffset = ((uint_t)VirtAddr - pDrvBuf->virtBase) >> PAGE_SHIFT;

    WaitResult wr;
    pDrvBuf->pBufferLock->Lock(&wr);

    Drv_Assert((pDrvBuf->uBitMap[uPageoffset >> 5] >> (uPageoffset & 0x1F)) & 0x1);
    ChangeMemMap(pDrvBuf->uBitMap, uPageoffset >> 5, uPageoffset & 0x1F, uPages);
    pDrvBuf->uFreePages += uPages;

    if (pDrvBuf == s_pDrvBufFast) {
        if (pDrvBuf->uTotalPages == pDrvBuf->uFreePages) {
            CKEN &= ~CLK_SRAM; // Close SRAM clock
        }
    }

    pDrvBuf->pBufferLock->Unlock();

    DBGOUT(DumpMemMaps());
    DBGOUT(kprintf("Finish Drv_FreeBuffer\n"));

    return;
}

EXTERN_C uint_t Drv_GetBufferPhysicalAddress(void * VirtAddr)
{
    DrvBuffer *pDrvBuf = GetDrvBuf(VirtAddr);
    uint_t PhysAddr = ((uint_t)VirtAddr - pDrvBuf->virtBase) + pDrvBuf->physBase;

    DBGOUT(kprintf("Drv_GetBufferPhysicalAddress: "));
    DBGOUT(kprintf("VirAddr = 0x%08x, PhysAddr = 0x%08x\n", VirtAddr, PhysAddr));
    return PhysAddr;
}
